#include <Arduino.h>
#include <ESP32Servo.h>
#include<WiFi.h>
#include <WiFiClient.h>
#include "minikame.h"
#include "emotion.h"
const char* ssid = "HUAWEI_29BC";
const char* password = "AbC971013.";
// 服务器信息
const char* server_address = "49.233.245.232";
const int server_port = 8888;

void parseData(String data);
void emotionData(String data);


MiniKame robot;
WiFiClient client;
bool running=0;
bool flag = 0;
String input="0";
String emotion="0";
int _time = 0; 
bool delays = 0;


void TASK_ONE(void *param );
void TASK_TWO(void *param );

void setup() {
    
    Serial.begin(115200);
    delay(1000);
    robot.init();
    WiFi.begin(ssid, password);
    while (WiFi.status() != WL_CONNECTED) 
    {
      delay(1000);
      Serial.println("正在连接 WiFi...");
    }
    Serial.println("WiFi 连接成功");
//   接收响应数据
    Link();
    Serial.println("服务端连接成功"); 
     xTaskCreatePinnedToCore(
                    TASK_ONE,          /* 任务函数 */
                    "TaskOne",         /* 任务名 */
                    8*1024,            /* 任务栈大小，根据需要自行设置*/
                    NULL,              /* 参数，入参为空 */
                    1,                 /* 优先级 */
                    NULL,0);  
     xTaskCreatePinnedToCore(
                    TASK_TWO,          /* 任务函数 */
                    "TaskTwo",         /* 任务名 */
                    8*1024,            /* 任务栈大小*/
                    NULL,      /* 参数为变量，传递入参时,需传递该变量的地址并强转为void *型，否则无法传递，编译报错 */
                    1,                 /* 优先级 */
                    NULL,1);  
u8g2.begin();
    u8g2.enableUTF8Print();
}
void Link()
{
  // 建立 TCP 连接
  IPAddress server_ip;
  while (!server_ip.fromString(server_address)) {
    delay(1000);
    Serial.println("正在解析服务器 IP 地址...");
  }
  Serial.print("服务器 IP 地址: ");
  Serial.println(server_ip);

  while (!client.connect(server_ip, server_port)) {
    delay(1000);
    Serial.println("正在连接服务器...");
  }
  Serial.println("已连接服务器");
  
  // 发送数据
  String data = "6_0_-1_-1#";
  client.println(data);
}

void loop()
{
  u8g2.setFont(u8g2_font_unifont_t_symbols);
  u8g2.firstPage();
  do
  {
    if(emotion=="255")
    {
      emotionData("30");
      break;
    }
    emotionData(emotion);
  } while (u8g2.nextPage());
  delay(500);
}
void parseData(String data){
    
      if(data =="1")
      {
          //normal_emotion_1();
          robot.walk(1,1000);//走
          //robot.run(1,800);
          Serial.println("walk");
          running = 1;
      }
      else if(data =="2")// 跑
      {
        robot.run(1,500);//550
        running = 1;
        Serial.println("run");
       
      }
      else if(data =="3")
      {// Left
          robot.turnL(1,400);//左转
          Serial.println("turnL");
          running = 1;
          if(_time>2)
          {
            running = 0;
            input = "0";
            _time=0;
          }
          else
          {
            _time=_time+1;
            //Serial.println(_time);
            running = 1;
          }
      }
      else if(data=="4")
      {
     // Right
          robot.turnR(1,400);//右转
          Serial.println("turnR");
          running = 1;
          if(_time>2)
          {
            running = 0;
            input = "0";
            _time=0;
          }
          else
          {
            _time=_time+1;
            //Serial.println(_time);
            running = 1;
          }
      }

      else if(data=="5")
      {
          running = 0;//停止
          Serial.println("STOP");
      }

      else if(data=="6")//俯卧撑
      {
          robot.pushUp(2,1500);
          running = 1;
          if(_time>=3)
          {
            running = 0;
            input = "0";
            _time=0;
          }
          else
          {
            _time=_time+1;
            //Serial.println(_time);
            running = 1;
          }
          Serial.println("push up");
      }

      else if(data=="7")//跳跃
      {
          robot.upDown(4,700);
          running = 1;
           if(_time>3)
          {
            running = 0;
            input = "0";
            _time=0;
          }
          else
          {
            _time=_time+1;
            //Serial.println(_time);
            running = 1;
          }
          Serial.println("updown");
      }

      else if(data=="8")//蹲跳
      {
          robot.jump();
          running = 1;
          Serial.println("jump");
//          if(_time>5)
//          {
//            running = 0;
//            input = "0";
//            _time=0;
//          }
//          else
//          {
//            _time=_time+1;
//            //Serial.println(_time);
//            running = 1;
//          }
      }

      else if(data=="9")//打招呼
      {
        if(flag)
        {
          robot.hello();
          flag = 0;
        }
          running = 0;
          Serial.println("say hello");
      }

      else if(data =="10") // punch
      {
          robot.frontBack(4,800);//200
          Serial.println("frontBack");
          running = 0;
      }
      else if(data=="11")//跳舞
      {
        robot.dance(2,1200);
        running = 1;
        if(_time>=3)
          {
            running = 0;
            input = "0";
            _time=0;
          }
          else
          {
            _time=_time+1;
            //Serial.println(_time);
            running = 1;
         }
        Serial.println("dance");
      }
      else if(data=="12")//mini步
      {
        robot.omniWalk(1,500,false,20);//550
        running = 1;
         if(_time>=3)
          {
            running = 0;
            input = "0";
            _time=0;
          }
          else
          {
            _time=_time+1;
            //Serial.println(_time);
            running = 1;
         }
        Serial.println("omniWalk");
      }
      else if(data=="13")//太空步U
      {
        
        running = 1;
        if(_time<5)
          {
            running = 1;
            robot.moonwalkL(2,2000);//2000
            _time=_time+1;
          }
          else if (_time <9)
          {
            
            robot.turnR(1,400);//右转
            //Serial.println("turnR");
            running = 1;
            _time=_time+1;
          }
          else if(_time<14)
          {
            running = 1;
            robot.moonwalkL(2,2000);//2000
            _time=_time+1;
          }
          else
          {
            running =0;
            input = "0";
             _time = 0;
            }
          
        Serial.println("moonwalkL");
      }
      else if(data=="14")//趴下
      {
        robot.getDown();
        //Serial.println("get Down");
        running = 1;
      }
      else if(data=="15")//坐下
      {
        robot.sitDown();
        //Serial.println("sit down");
        running = 1;
      }
      else if(data=="16")//握手
      {
        if(flag)
        { 
          robot.home();
          flag=0;
          delay(500);
        }
        robot.shakeHand();
        Serial.println("shakehand");
        running = 1;
        if(_time>150)
          {
            running = 0;
            input = "0";
            _time=0;
          }
          else
          {
            _time=_time+1;
            //Serial.println(_time);
            running = 1;
         }
        }
      else if(data=="17")//碰拳
        {
          if(flag)
          {
            robot.home();
            flag=0;
            delay(500);
          }
          robot.bumpFisrs();
          Serial.println("bumpFisrs"); 
          running = 1;
          
          
          if(_time>150)
          {
            running = 0;
            input = "0";
            _time=0;
          }
          else
          {
            _time=_time+1;
            //Serial.println(_time);
            running = 1;
         }
        }
        else if(data=="18")//左转圈
        {

          robot.turnL(1,400);
          Serial.println("REVOLVE_L");
          running = 1;
          if(_time>=13)
          {
            running = 0;
            input = "0";
            _time=0;
          }
          else
          {
            _time=_time+1;
            //Serial.println(_time);
            running = 1;
          }
         }
        else if(data=="19")//右转圈
        {

          robot.turnR(1,400);
          Serial.println("REVOLVE_R");
          if(_time>13)
          {
            running = 0;
            input = "0";
            _time=0;
          }
          else
          {
            _time=_time+1;
            //Serial.println(_time);
            running = 1;
          }
        }
       else if(data =="20")// 后退
      {
        robot.runBack(1,500);//550
       
        Serial.println("runBack");
          if(_time>3)
          {
            running = 0;
            input = "0";
            _time=0;
          }
          else
          {
            _time=_time+1;
            //Serial.println(_time);
            running = 1;
          }
       
      }
      else if(data =="21")// 过来
      {
        robot.run(1,500);//550
       
        Serial.println("near");
          if(_time>3)
          {
            input = "0";
            _time=0;
          }
          else
          {
            _time=_time+1;
            //Serial.println(_time);
            running = 1;
          }
       
      }
      else if(data =="22")
      {// TurnBack
          robot.turnL(1,400);//左转
          Serial.println("turnL");
          running = 1;
          if(_time>=6)
          {
            running = 0;
            input = "0";
            _time=0;
          }
          else
          {
            _time=_time+1;
            //Serial.println(_time);
            running = 1;
          }
      }
      else if(data=="0")//回到初始状态
      {
        robot.home();
        running=1;
      }
      
      else{
          robot.home();
//        Serial.println("home");
     }
 }


void emotionData(String data){
    
      if(data =="1")
      {
          expect_emotion();
          Serial.println("期待");
      }
      else if(data =="2")//
      {
        stunned_emotion();
        Serial.println("惊呆");
       
      }
      else if(data =="3")
      {
          confusion_emotion();
          Serial.println("困惑");
      }
      else if(data=="4")
      {
          happy_emotion();//
          Serial.println("开心");    
      }

      else if(data=="0")
      {
          normal_emotion_1();
          Serial.println("正常");
      }

      else if(data=="6")//喜欢
      {
          love_emotion();
          Serial.println("喜欢");
      }

      else if(data=="7")//
      {
          sad_emotion();
          Serial.println("悲伤");
      }

      else if(data=="8")
      {
          speechless_emotion();
          Serial.println("无语");
      }

      else if(data=="9")//打招呼
      {
          hello_emotion();
          Serial.println("say hello");
      }

      else if(data =="10") 
      {
          anger_emotion();//200
          Serial.println("生气");
      }
      else if(data=="11")
      {
        pain_emotion();
        Serial.println("疼痛");
      }
      else if(data=="12")
      {
        exhausted_emotion();
        Serial.println("疲惫");
      }
      else if(data=="13")
      {
        grievance_emotion();//2000
        running = 1;
        Serial.println("委屈");
      }
      else if(data=="14")
      {
        enjoy_emotion();
        Serial.println("享受");
      }
      else if(data=="15")
      {
        vertigo_emotion();
        Serial.println("眩晕");
      }
      else if(data=="5")
      {
        indifferent_emotion();
        Serial.println("冷漠");
      }
      else{
           normal_emotion_2();
          Serial.println("正常");
     }
 }
void TASK_ONE(void *param )
{
  while(1)
  {
    if(running)
      parseData(input);
    else
      parseData("0");
    if(input=="50")
     { 
        String st = "0_0_-1_-1#";
        client.println(st);
        break;
     }
     delay(20);
     //_time=_time+1;
     //
  }
}
void TASK_TWO(void *param ){
  while (client.connected()) {
                if (client.available()) {
                    while(client.available()) {
                    String desion = client.readStringUntil('\n');
                    input = "0";
                    emotion = "0";
                    Serial.println(desion);
                    if(desion.length()>0)
                    {
                      int x = desion.indexOf("_",0);
                      input = desion.substring(0,x);
                      Serial.println(input);
                      int y = desion.indexOf("_",x+1);
                      emotion = desion.substring(x+1,y); 
                      Serial.println(emotion);
                    }
                    running = 1;
                    flag = 1;
                    _time = 0;
                    //delays = 1;
                    Serial.println("接收数据成功");
                }
                }
                else robot.home();
            delay(50);
          }
        input="50";
  }